/*
 * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include "ros/ros.h"
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
// %EndTag(MSG_HEADER)%

#include <sstream>
#include <string.h>   /* ERROR Number Definitions           */
#include "SerialPort_write.h"   /* ERROR Number Definitions           */

const unsigned char yun_stop[7]= {0xFF ,0x01 ,0x00 ,0x00 ,0x00 ,0x00 ,0x01};
const unsigned char yun_up[7]=   {0xFF ,0x01 ,0x00 ,0x08 ,0x00 ,0x20 ,0x29};
const unsigned char yun_down[7]= {0xFF ,0x01 ,0x00 ,0x10 ,0x00 ,0x20 ,0x31};
const unsigned char yun_left[7]= {0xFF ,0x01 ,0x00 ,0x04 ,0x20 ,0x00 ,0x25};
const unsigned char yun_right[7]={0xFF ,0x01 ,0x00 ,0x02 ,0x20 ,0x00 ,0x23};

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
//void chatterCallback(geometry_msgs::Twist)
void chatterCallback(geometry_msgs::Twist msg)
{
  //ROS_INFO("I heard: [%s]", msg->data.c_str());
  if(msg.linear.x==1)
  {
      //ROS_INFO("Yes i heard something that it is so weird! --11111111111111111111111111--");
      ROS_INFO("up");
      send_data(yun_up,7);
  }
  else if(msg.linear.x==2)
  {
      //ROS_INFO("Yes i heard something that it is so weird! --22222222222222222222222222--");
      ROS_INFO("left");
      send_data(yun_left,7);
  }
  else if(msg.linear.x==3)
  {
      //ROS_INFO("Yes i heard something that it is so weird! --33333333333333333333333333--");
      ROS_INFO("down");
      send_data(yun_down,7);
  }
  else if(msg.linear.x==4)
  {
      //ROS_INFO("Yes i heard something that it is so weird! --44444444444444444444444444--");
      ROS_INFO("right");
      send_data(yun_right,7);
  }
  else if(msg.linear.x==5)
  {
      //ROS_INFO("Yes i heard something that it is so weird! --%f--",msg.linear.x);
      ROS_INFO("stop");
      send_data(yun_stop,7);
  }
}

int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe<geometry_msgs::Twist>("chatter", 1000, chatterCallback);
  //open_serial();

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  ros::spin();

  return 0;
}
